#include "engine.h"
#include "meshcollider.h"
#include "render/rendernode.h"
#include "intersection.h"
#include "render/rendermaterial.h"

void meshcollider_t::generate(rendernode_t* rn)
{
	ASSERTTXT(rn->mesh,"HULYE VAGY?");

	rendermesh_t* mesh=rn->mesh;
	

	for (uint32 n=0; n<mesh->submesh.size(); ++n)
	{
		submesh_t& sm=mesh->submesh[n];
		if (sm.material->name=="alphatest")
			continue;
		char* vb=(char*)sm.vertexbuffer.lock_read();
		uint16* ib=sm.indexbuffer.lock_read();
		uint32 posptr=pos.size();
		uint32 triptr=tri.size();

		pos.resize(posptr+sm.vertexbuffer.vertexnum);
		tri.resize(triptr+sm.indexbuffer.numindices/3);

		for (uint32 i=0; i<sm.indexbuffer.numindices/3; ++i)
		{
			tri[triptr+i].i1=ib[0]+posptr;
			tri[triptr+i].i2=ib[1]+posptr;
			tri[triptr+i].i3=ib[2]+posptr;

			ib+=3;
		}

		for (uint32 v=0; v<sm.vertexbuffer.vertexnum; ++v)
		{
			pos[posptr+v]=*(float3*)vb;
			vb+=sm.vertexbuffer.vertexsize;
		}

		sm.vertexbuffer.unlock();
		sm.indexbuffer.unlock();
	}

	if (tri.size())
		generate_aabb(0,0,tri.size()-1,-1);
}

struct midstruct_t
{
	float3 p;
	uint16 i;
};

template<int n>
struct trisort_t
{
	bool operator()(const midstruct_t& v1, const midstruct_t& v2)  const
	{
		return v1.p[n]<v2.p[n];
	}
};



void meshcollider_t::generate_aabb(uint32 aabb_index,uint32 firsttri, uint32 lasttri, int lastsort)
{
	uint32 trinum=lasttri-firsttri+1;
	aabb bound;
	bound.init();

	vector<midstruct_t> midbufvec;
	midbufvec.resize(trinum);
	midstruct_t* midbuf=&midbufvec[0];

	for (uint32 n=firsttri; n<=lasttri; ++n)
	{
		bound.extend(pos[tri[n].i1]);
		bound.extend(pos[tri[n].i2]);
		bound.extend(pos[tri[n].i3]);

		midbuf[n-firsttri].p=(pos[tri[n].i1]+pos[tri[n].i2]+pos[tri[n].i3])/3;
		midbuf[n-firsttri].i=n;
	}

	float3 extent=bound.max-bound.min;
	bounding.resize(max(bounding.size(),aabb_index+1));
	bounding[aabb_index]=bound;

	if (trinum<=10)
	{
		return;
	}

	int i=0;
	if (extent.y>extent.x)
		i=1;
	if (extent.z>extent[i])
		i=2;



	if (lastsort!=i)
	{
		switch(i)
		{
		case 0:
			sort(midbuf,midbuf+trinum,trisort_t<0>());
			break;
		case 1:
			sort(midbuf,midbuf+trinum,trisort_t<1>());
			break;
		case 2:
			sort(midbuf,midbuf+trinum,trisort_t<2>());
			break;
		}
	}

	vector<triangle_t> tmpvec;
	tmpvec.resize(trinum);
	triangle_t* tmp=&tmpvec[0];
	memcpy(tmp,&tri[firsttri],trinum*sizeof(triangle_t));

	for (uint32 n=firsttri; n<=lasttri; ++n)
		tri[n]=tmp[midbuf[n-firsttri].i-firsttri];


	uint32 midelem=(firsttri+lasttri)/2;
	generate_aabb(2*aabb_index+1,firsttri,midelem,i);
	generate_aabb(2*aabb_index+2,midelem+1,lasttri,i);
}


bool meshcollider_t::intersect_sphere(float3& hit_pos, float3& hit_normal,const float4x4& parentmtx, const float3& center,float radius)
{
	float4x4 node_world; node_world.mul(mtx,parentmtx);
	float4x4 imtx; imtx.invert(node_world);
	float3 relcenter=mul4x3(center,imtx);

	float r=radius/length(node_world.axis(0));
	float tmin=10*r;
	float3 tripos;
	float3 trinormal;

	intersect_sphere_rec(0,0,tri.size()-1,tmin,tripos,trinormal,relcenter,r);
	if (tmin<5*r)
	{
		float3 abstripos=mul4x3(tripos,node_world);
		hit_normal=mul3x3(trinormal,node_world);
		float3 d=(center-abstripos); d.normalize();
		hit_pos=abstripos+radius*d;
		return true;
	}
	return false;
}

void meshcollider_t::intersect_sphere_rec(uint32 aabb_index,uint32 firsttri,uint32 lasttri,float& tmin, float3& tripos,float3& trinormal,const float3& relcenter, float radius)
{
	uint32 trinum=lasttri+1-firsttri;

	if (trinum<=10)
	{
		for (uint32 n=firsttri; n<=lasttri; ++n)
		{
			uint16 i1=tri[n].i1;
			uint16 i2=tri[n].i2;	
			uint16 i3=tri[n].i3;

			float3 v1=pos[i1];
			float3 v2=pos[i2];
			float3 v3=pos[i3];

			float3 cp=ClosestPtPointTriangle(relcenter,v1,v2,v3);

			float t2=length2(cp-relcenter);
			if (t2<sqr(radius))
			{
				if (t2<sqr(tmin))
				{
					tmin=sqrtf(t2);
					tripos=cp;
					trinormal.cross(v2-v1,v3-v1);
					trinormal.normalize();
				}
			}
		}
	}
	else
	{
		if (sphere_aabb_intersect(relcenter,radius,bounding[aabb_index].min,bounding[aabb_index].max))
		{
			uint32 midtri=(firsttri+lasttri)/2;
			intersect_sphere_rec(2*aabb_index+1,firsttri,midtri,tmin,tripos,trinormal,relcenter,radius);
			intersect_sphere_rec(2*aabb_index+2,midtri+1,lasttri,tmin,tripos,trinormal,relcenter,radius);
		}
	}
}

void generate_meshcolliders(vector<rendernode_t>& nodes,vector<meshcollider_t>& colliders)
{
	colliders.clear();
	nodes[0].local_matrix.identity();
	nodes[0].world_matrix.identity();

	for (unsigned n=1;n<nodes.size(); ++n)
	{
		rendernode_t* node=&nodes[n];
		node->world_matrix.mul(node->local_matrix,nodes[node->parent_index].world_matrix);

		if (node->mesh)
		{
			colliders.resize(colliders.size()+1);
			colliders[colliders.size()-1].mtx=node->world_matrix;
			colliders[colliders.size()-1].generate(node);
			if (!colliders[colliders.size()-1].tri.size())
				colliders.pop_back();
		}
	}
}